/*
 * @Author       : HONGKELI lihongkje@outlook.com
 * @Date         : 2024-04-24 09:33:47
 * @LastEditors  : HONGKELI lihongkje@outlook.com
 * @LastEditTime : 2024-04-25 10:03:29
 * @FilePath     : /EngMath/src/Models/Plane.cpp
 * @Description  : 
 */

#include <cmath>
#include <EngMath/Models/Plane.h>

void EM::Plane::compute_point_to_plane_distance(const EM::Vector3d &point, double &distance) const
{
    // Get the vector AP, 
        // which is formed by the fixed point on the plane and the input point from the function parameter
    EM::Vector3d vector_AP = this->point_ - point;

    // Get dot product 
        // vector AP with vector normal_
    double double_dot = vector_AP.dot(this->normal_);

    // Get distance
    distance = std::abs(double_dot);
}

void EM::Plane::compute_flatness_average_distance
    (const std::vector<Vector3d> &points, double &double_flatness) const
{
    /***********************************************************
     * This is the simplest method for cakculating planarity,
     *  it is merely taking the average of the distances from the set of points to the plane.
     ***********************************************************/

    double double_distance_gather = 0;
    double double_distance_it = 0;

    for(Vector3d point_it : points){
      this->compute_point_to_plane_distance(point_it, double_distance_it);
      double_distance_gather += double_distance_it;
    }

    // get flatness 
    double_flatness = double_distance_gather / points.size();
}

void EM::Plane::compute_flatness_standard_deviation_distance
    (const std::vector<Vector3d> &points, double &double_flatness) const
{
    // 1.  Calculate the mean distance from points to the plane. 
    double double_average_distance = 0;
    this->compute_flatness_average_distance( points, double_flatness );
    
    // 2. Calculate the variance of distances.
    double variance = 0;
    for (const Vector3d &point : points) {
        double distance;
        this->compute_point_to_plane_distance(point, distance);
        double deviation = distance - double_average_distance;
        variance += deviation * deviation;
    }
    variance /= points.size();

    // 3. Standard deviation is the square root of variance.
    double_flatness = sqrt(variance);
}

void EM::Plane::compute_flatness_RMS_distance
    (const std::vector<Vector3d> &points,double &double_flatness) const
{
    

}


void EM::Plane::compute_degree(const Plane &other, double &double_degree) const
{
    double double_dotProduct = normal_.dot(other.normal_);
    // get norms
    double double_norms = normal_.norm() * other.normal_.norm();
    // get radian
    double double_radian = acos(double_dotProduct / double_norms);
    // get radian to degree
    double_degree = double_radian * (180.0 / M_PI);
}